#include <iostream>
#include <vector>
#include <opencv2/opencv.hpp>
#include "cmvutils.h"
#include "cmvvision.h"

static const bool CAMERA = true;
static const bool KINECT = false;
static const bool PREVIEW = false;
static const bool TEST = true;

int main()
{
	std::vector<cmv::Color> colors;
	cmv::Vision *vision;
	cmv::ImageYUV422 image;
	cv::Mat bgrMat;
	cv::Mat pointMat;
	std::vector<cmv::Point> points;
	bool initialized = false;

	colors = cmv::loadConfig("colors.txt");
	vision = new cmv::Vision; // 栈空间不够，使用堆
	cv::namedWindow("image", CV_WINDOW_AUTOSIZE);

	while (true) {
		if (CAMERA) {
			// 从摄像头中读取
			static cv::VideoCapture capture(0);
			if (!capture.isOpened()) {
				std::cerr << "can't open camera!" << std::endl;
				return -1;
			}
			capture >> bgrMat;
		} else if (KINECT) {
			// 从Kinect读取
			static cv::VideoCapture capture(CV_CAP_OPENNI);
			if (!capture.isOpened()) {
				std::cerr << "can't open kinect!" << std::endl;
				return -1;
			}
			capture.grab();
			capture.retrieve(bgrMat, CV_CAP_OPENNI_BGR_IMAGE);
			capture.retrieve(pointMat, CV_CAP_OPENNI_POINT_CLOUD_MAP);
		} else {
			// 从文件中读取
			bgrMat = cv::imread("a.jpg");
		}

		if (!cmv::loadImageYUV422FromMat(bgrMat, image)) {
			continue;
		}
		if (!initialized) {
			vision->initialize(colors, image.width, image.height);
			initialized = true;
		}
	
		if (PREVIEW) {
			// 预览
			vision->testClassify(image.data, bgrMat.data);
		} else if (TEST) {
			vision->testProcess(image.data, bgrMat.data, (float *)pointMat.data, &points);
		} else {
			// 框出结果
			vision->processFrame(image.data);
			for (std::vector<cmv::Color>::size_type i = 0; i < colors.size(); ++i) {
				cmv::Region *region = vision->getRegions(i);
				while (region) {
					if (region->area > 1000) {
						cv::rectangle(bgrMat, cv::Point(region->x1, region->y1), cv::Point(region->x2, region->y2),
								cv::Scalar(colors[i].color_b, colors[i].color_g, colors[i].color_r),
								5);
					}
					region = region->next;
				}
			}
		}

		cv::imshow("image", bgrMat);
		if (cv::waitKey(30) != -1) {
			break;
		}
	}

	delete vision;
	return 0;
}
